import numpy as np
from riccati import solve_care

aq = 10, 0, 10, 0, 10
r = 0.1
mA=np.array([
    [0, 1, 0, 0, 0],
    [11, 0, 1, 0, 0],
    [0, 0, 0, 1, 0],
    [0.5, 0, 5.5, 0, 0],
    [1, 0, 1, 0, 0]
    ])
mB=np.array([[0], [0.1], [0], [0.05], [0.1]])
mQ=np.zeros([5, 5])
for n in range(5):
    mQ[n, n] = aq[n]
mR=np.array([r])
mP = solve_care(mA,  mB,  mQ,  mR)
K = -1/r * np.dot(mB.T,  mP)
K = K[0]
for n in range(K.shape[0]):
    print(K[n],  end=',  ')
